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EXPLOITING  CAPABILITY  CONSTRAINTS  TO  SOLVE  GLOBAL, 
TWO  DIMENSIONAL  PATB  PLANNING  PROBLEMSf 


R.  F.  Richbourg,  Neil  C.  Rowe,  Michael  J.  Zyda 
Department  of  Computer  Science,  Naval  Postgraduate  School,  Monterey,  California 

Abstract 

Mobile  autonomous  vehicles  require  the  capability  of  planning  routes  over  ranges  that  are 
too  great  to  be  characterized  by  local  sensor  systems.  Completion  of  this  task  requires  some  form 
of  map  data.  Much  work  has  been  done  concerning  planning  paths  through  local  areas,  those 
which  can  be  scanned  by  on-board  sensor  systems.  However,  planning  paths  based  on  long  range 
map  data  is  a  very  different  problem  Extant  solution  techniques  require  the  search  of  discrete, 
node  and  link  representations  which  characterize  continuous,  two  dimensional  problem  environ- 
ments We  assume  the  availability  of  topographic  data  organized  into  regions  of  homogeneous 
traversal  cost.  Given  this,  we  present  a  solution  technique  for  the  long  range  planning  problem 
which  relies  on  a  Snell's  Law  heuristic  to  limit  a  graph  search  for  the  optimal  solution. 

1.    Introduction 

Route  planning  has  been  an  area  of  interest  for  many  years,  particularly  in  the  Operations 
Research  field  Ref.  1].  Much  effort  has  been  directed  towards  the  problem  of  computing  optimal 
paths  through  shipping  lanes,  road  networks,  and  other  similarly  described,  static  media  These 
problems  feature  a  discrete  number  of  choices  for  possible  paths  and  can  thus  be  characterized  as 
essentially  one  dimensional.  Recently,  interest  in  two  dimensional  route  planning  has  grown.  In 
this  domain,  there  is  a  continuum  of  available  choices  for  the  next  movement.  Such  planning  is 
appropriate  for  robot  or  human  cross  country  routing  or  for  determining  placements  for  the  future 
construction  of  permanent,  linear  features  such  as  pipelines  or  roads.  The  simplest  version  of  this 
problem  is  known  as  the  Find-Path  problem    Ref.  2  . 


f   This  work  has  been  supported  by  the  NPS  Foundation  Research  Program.     This  paper  will  appear  in  the 
Proceedings  of  the  1986  IEEE  International  Conference  on  Robotics  and  Automation 


A  solution  to  the  classic  Find-Path  problem  consists  of  a  routing  plan  tailored  to  a  specific 
environment.  In  the  robotics  area,  the  problem  is  described  as,  given  the  initial  and  goal  locations 
of  an  object,  and  a  set  of  obstacles  in  the  environment,  find  a  continuous  path  for  the  movement 
of  the  object  from  the  initial  location  to  the  goal  location  which  avoids  collisions  with  any  obsta- 
cles along  the  way  |Ref  2  The  problem  has  typically  been  associated  with  planning  local  motion 
control  or  manipulator  trajectory  control  for  a  robot  in  a  two  or  three  dimensional  space  where 
information  is  gained  through  integral  sensor  systems,  primarily  visual. 

There  is  a  version  of  the  classic  Find-Path  problem  which  requires  planning  motion  over 
ranges  too  great  to  be  explored  by  local  sensor  systems.  We  term  this  problem  the  extended 
lind-Palh  problem  bong  range  planning  is  a  useful  and  difficult  problem  which  humans  solve 
easily  many  times  each  day.  Prior  to  initiating  any  movement,  humans,  either  consciously  or  not, 
plan  a  particular  course  to  be  followed.  Moreover,  the  planned  course  has  the  characteristic  that 
some  factor  is  optimized  (or  at  least  perceived  as  optimized).  A  typical  optimization  factor  is  that 
of  time. 

A  salient  difference  between  the  classic  and  extended  Find-Path  problems  concerns  optimi- 
zation The  classic  version  posits  a  binary  view  of  the  world.  Every  point  in  the  environment  is 
either  traversable  or  impassable  An  optimal  route  in  such  an  environment  can  be  found  by  plan- 
ning the  shortest  distance  path  around  the  obstacles.  This  "least  distance  implies  least  cost" 
assumption  is  viable  in  the  classic  version  because  the  range  of  the  sensor  systems  generally 
encompasses  only  a  uniform  medium  of  traversable  areas.  The  validity  of  this  assumption  can 
easily  be  established  by  an  example  (consider  time  as  a  measure  of  the  cost  of  a  route).  Clearly, 
one  can  walk  across  the  floor  ten  feet  away  as  quickly  as  one  can  cross  the  same  floor  only  two 
feet  away,  given  the  absence  of  obstacles 

The  "least  distance  implies  least  cost"  assumption  is  not  reasonable  for  the  extended  Find- 
Path  problem  because  the  increased  physical  range  over  which  travel  occurs  generally  implies  that 
more  than  one  traversable  medium  is  represented  Regardless  of  the  factor  which  is  to  be  optim- 
ized,  one   medium   can   not    be  assumed   to   have  the  same  traversal   characteristics  as  all  others. 
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given  the  diversity  of  objects  which  can  be  found  in  the  physical  world.  As  the  cost  of  traversing 
one  area  can  be  quite  different  from  those  of  other  areas,  a  new  parameter  is  introduced  into  the 

cost  computation  for  each  possible  path    In  the  binary  case  problem,  the  cost  of  traversing  every 

n 

route  is  computed  by  the  simple  formula  Cost  ~  Yl^>    wnere  there  are  n  line  segments  in  the  com- 

•  -  ] 

plete  path  and  d,    is  the  Euclidian  distance  along  the  i'th  line  segment.  Considering  the  extended 

n 

problem,  the  formula  becomes  Cost  =  J^c,  x  d,    where  n    and  d,    have  the  same  meaning    The  new 

i  =  1 

parameter,  c,  reflects  the  cost  of  the  i'th  line  segment  Also,  the  number  of  line  segments  is  typi- 
cally increased  As  an  example,  a  straight  line  traversing  two  different  cost  areas  is  represented  by 
two  line  segments.  Only  one  line  is  required  to  reflect  the  same  situation  in  a  binary  case 
representation.  The  addition  of  cost  information  has  the  effect  of  invalidating  the  straight  line 
hypothesis  so  prevalent  in  the  binary  case  solution  techniques. 

1.1.    Brief  Review  of  Some  Extant  Solution  Techniques 

The  prototypical  solution  technique  for  the  binary  case  problem  is  known  as  the  Visibility 
Graph  or  VGraph  method  Ref  7,8] .  This  method  relies  on  the  knowledge  that  the  optimal  path 
must  be  either  a  straight  start  to  goal  line  or  a  series  of  line  segments  which  includes  obstacle  ver- 
tices as  turn  points  along  the  start  to  goal  path.  The  VGraph  technique  creates  a  graph  represen- 
tation of  the  problem,  G  =  (V,L),  where  the  set  V  includes  all  obstacle  vertices  plus  the  start  and 
goal  locations.  The  straight  line  hypothesis  is  used  to  create  the  set  L.  Whenever  an  unobstructed 
line  segment  connects  any  two  members  of  V,  that  segment  is  included  in  L.  When  the  construc- 
tion of  the  graph  is  complete,  standard  graph  search  techniques  can  be  used  to  find  the  optimal 
path  in  G. 

The  exhaustive  nature  of  G  defeats  the  interacting  subproblems  difficulty  of  the  Find-Path 
problem.  The  cost  is  that  every  obstacle  in  the  problem  environment  must  be  considered.  When  o 

obstacles  are  present,   having  a  total  of  v  vertices,  the  cardinality  of  V  is  n  -  v  -  2.   In  the  worst 

i  =  n  -  1 
case,      £]    i    line  segments  must  be  inspected  to  form  L.  This  can  be  expensive  for  large  problem 


spaces.  A  dynamically  created  graph  considering  only  those  obstacles  in  the  general  area  of  the 
optimal  path  is  less  costly.  However,  care  must  be  taken  to  insure  a  global  problem  view  so  that 
interacting  subproblems  do  not  prevent  optimal  solutions. 

The  wavefront  propagation  technique  is  the  most  common  method  used  to  solve  the 
extended  problem  jRef.  5,6,11,12).  In  this  technique,  a  uniformly  spaced  lattice  is  imposed  on  the 
problem  representation  Each  link  is  assigned  a  cost  relative  to  the  actual  cost  of  traversing  the 
associated  real  world  terrain.  Then,  an  omnidirectional  graph  search,  similar  to  the  propagation  of 
a  wave,  is  conducted.  When  the  wavefront  touches  the  goal,  the  optimal  path  can  be  retrieved  by 
referencing  backpointers  or  a  similar  technique.  This  method  has  the  primary  advantage  that  it  is 
the  only  known  technique  which  provides  optimal  solutions  to  the  extended  problem.  However,  it 
has  significant  deficiencies. 

The  propagation  technique  is  very  expensive  Much  of  the  search  is  expended  in  areas  that 
do  not  hold  any  portion  of  the  optimal  path  since  there  is  no  guiding  principle  such  as  the 
straight  line  hypothesis  used  in  the  binary  case  problem  Also,  there  is  a  digital  bias  inherent  in 
the  problem  representation  so  that  the  method  actually  returns  a  set  of  "optimal"  paths,  each  of 
which  must  be  compared  to  determine  the  unique  solution  Ref.  6  .  The  method  finds  "stairstep" 
approximations  to  straight  line  solutions.  These  must  be  smoothed  out  to  characterize  the  optimal 
path. 

One  technique  which  attempts  to  avoid  the  exhaustive  lattice  of  the  propagation  technique 
is  the  homogeneous  regions  approach  |Ref.  3j.  This  method  relies  on  the  knowledge  that  the  phy- 
sical world  is  not  a  series  of  disjoint  points,  but  rather  a  grouping  of  similar  regions  (fields,  lakes, 
etc  )  The  points  inside  each  region  have  equivalent  traversal  characteristics.  The  method  operates 
by  representing  the  problem  space  as  a  grouping  of  centrally  symmetric  polygons  (homogeneous 
regions)  and  conducting  a  graph  search  between  region  centers  The  method  has  deficiencies:  not 
everything  in  the  physical  world  can  be  accurately  modeled  by  centrally  symmetric  figures  and 
paths  between  region  centers  need  not  include  the  best  routes  However,  the  observation  that  the 
real  world  is  a  grouping  of  regions,  not  distinct  points,  is  important 


2.    Desirable  Solution  Technique  Characteristics 

A  solution  technique  for  the  extended  Find-Path  problem  will  have  several  key  properties. 
First,  there  must  be  provisions  to  deal  with  the  interaction  of  subproblems.  Specific  domain 
knowledge  can  been  employed  to  prevent  these  difficulties  as  is  the  case  in  the  VGraph  model. 
Here,  knowledge  leads  to  the  exhaustive  decomposition  of  the  problem  into  a  graph  of  obstacle 
vertices  which  can  be  intelligently  searched.  The  analogous  problem  decomposition  for  the 
extended  problem  leads  to  the  imposition  of  a  uniform  lattice  structure  as  in  the  wavefront  propa- 
gation technique.  Again,  an  intelligent  graph  search  can  be  conducted  to  find  an  optimal  solution. 
The  salient  difference  is  that  an  unintelligent  problem  representation  confounds  the  search  stra- 
tegy and  leads  to  problems  of  representational  resolution,  combinatorial  explosion,  accumulation 
of  error,  and  a  multiplicity  of  solutions  which  erroneously  appear  as  equivalent.  The  homogeneous 
regions  approach  attempted  to  establish  a  more  intelligent  problem  representation  by  grouping 
similar  regions  together.  However,  this  technique  also  fails  due  to  poor  representational  robustness 
(not  all  physical  world  features  can  be  adequately  modeled)  and  the  lack  of  an  appropriate 
straight  line  hypothesis  to  guide  search  (moving  from  region  center  to  region  center  is  inade- 
quate). We  note  that  a  dynamically  created  graph  can  lead  to  greater  efficiency  by  avoiding 
wasteful  computations. 

We  noted  that  humans  solve  similar  problems  routinely.  They  are  able  to  provide  reason- 
able solutions  quite  efficiently.  Their  abilities  seem  to  rely  on  several  principles.  First,  they  make 
use  of  topographical  knowledge  and  knowledge  of  their  own  capabilities  to  partition  the  problem 
representation  into  homogeneous  areas  with  similar  traversal  characteristics.  Secondly,  humans 
reason  at  different  levels  of  abstraction.  A  high  level  route  plan  might  contain  the  step  "go 
through  Smithfield"  while  a  lower  level  abstraction  detailed  the  route  through  the  town  In  this 
sense,  abstraction  is  as  useful  in  combating  combinatorial  explosion  as  is  the  homogeneous  regions 
premise  Humans  are  also  opportunistic  Special  cases  of  terrain  features  present  opportunities  for 
problem  decomposition.  This  concept  generalizes  to  the  appearance  of  a  corridor  through  an  oth- 
erwise impenetrable  obstacle.   A  simple  example  is  a  door  in  a  building.  A   more  important  exam- 
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pie  is  the  occurrence  of  a  single  road  through  a  densely  wooded  and  treacherous  mountain  area.  A 
suitable  solution  technique  for  the  extended  Find-Path  problem  must  be  able  to  achieve  oppor- 
tunistic decomposition  by  recognizing  similar  situations.  Another  useful  aspect  of  human  reason- 
ing is  that  it  is  multidirectional.  Moreover,  directionality  is  intelligently  specified.  The  wavefront 
technique  is  multidirectional,  however,  omnidirectional  search  is  not  an  intelligent  strategy. 

In  summary,  a  suitable  solution  method  for  the  extended  problem  should  exhibit  several 
properties.  These  include  the  use  of  a  basic,  guiding  principle  for  search  (such  as  the  straight  line 
hypothesis),  domain  knowledge,  capability  knowledge,  multidirectional  (at  least  bidirectional)  and 
intelligent  search,  opportunistic  decomposition,  intelligent  problem  representation,  and  levels  of 
abstraction.  Also,  the  solution  provided  should  be  in  some  sense  optimal.*  \N  e  have  noted  that 
humans  quickly  solve  the  problem,  but  not  necessarily  with  optimal  results.  Graph  theoretic  tech- 
niques can  provide  optimal  solutions,  but  not  necessarily  quickly.  A  suitable  solution  method  for 
the  extended  Find-Path  problem  will  achieve  the  best  traits  of  both  methods. 

3.    Proposed  Solution  Technique 

We  have  observed  that  representing  the  problem  as  a  lattice  of  uniformly  spaced  nodes  pro- 
duces a  firm,  graph  theoretic  basis  from  which  an  optimal  solution  can  be  obtained.  However, 
obtaining  this  solution  is  computationally  expensive.  In  the  wavefront  propagation  technique,  the 
computational  cost  is  relative  to  the  distance  to  be  traveled  from  the  start  location  to  the  goal. 
The  number  of  grid  squares  examined  here  is  approximately  proportional  to  the  area  of  a  circle 
whose  radius  is  the  start  to  goal  distance.  Thus,  the  computational  cost  grows  approximately  in 
proportion  to  the  square  of  this  distance  (since  the  area  of  a  circle  is  tt  r  ).  Also,  a  technique  to 
intelligently  guide  the  wavefront  search  is  not  readily  apparent.  Because  of  the  diversity  of  speeds 
attainable  over  different  physical  terrain  features  and  the  arbitrary  nature  of  their  physical,  rela- 
tive placement,  it  is  generally  not  possible  to  determine  if  a  specific  area  is  more  favorable  to  the 


Optimality  ran  be  measured  by  many  factors  such  as  time,  fuel  used,  visibility,  danger  avoidance,  and  so  on. 
Another  consideration  is  the  amount  of  computation  required  to  obtain  the  solution  The  tradeoff  between  process- 
ing time  and  optimality  must  also  be  considered. 


-  7  - 

production  of  an  optimal  path  than  any  other  area,  unless  that  area  is  explicitly  moved  to  and 
examined  This  characteristic  causes  the  exhaustive,  omnidirectional  nature  of  the  wavefront  pro- 
pagation search.  We  desire  to  produce  a  method  which  constrains  the  search  required  of  wave- 
front  propagation  techniques.  Further,  the  developed  method  must  be  relatively  efficient  as  com- 
putational excess  is  the  primary  problem  to  be  overcome. 

Normally,  limiting  graph  searches  involves  finding  good  decision  criteria  for  use  at  branch 
points.  This  does  not  seem  appropriate  for  the  extended  problem  However,  we  can  bound  the 
physical  size  of  the  entire  graph  Such  a  bound  is  possible  and  easily  derived  in  the  following 
manner  Note  that  any  solution  to  the  problem  serves  as  an  upper  bound  on  the  cost  of  the 
optimal  solution  Consider  a  hypothetical  path  from  start  to  goal  which  traverses  only  the  least 
desirable  (i.e.  highest  cost)  regions  but  avoids  all  obstacles.  Compute  the  cost  of  this  hypothetical 
path  Now.  compute  the  distance  which  can  be  traversed  when  only  the  most  favorable  terrain 
type  is  used  and  the  same  cost  is  accrued  Consider  this  second  distance  to  be  the  sum  of  the  dis- 
tances from  each  foci  to  the  boundary  of  an  ellipse.  Let  the  start  and  goal  locations  define  these 
foci.  Clearly,  the  optimal  path  must  lie  entirely  within  this  ellipse.  Thus,  we  have  created  a  phy- 
sical bound  on  the  area  to  be  searched  by  a  propagation  technique  However,  the  bound  is  not 
tight.  Remembering  that  computational  cost  changes  in  approximate  relation  to  the  square  of  the 
distance  to  be  traveled,  we  note  that  obtaining  the  best  bound  possible  is  highly  desirable  There 
is  also  a  need  to  balance  the  "goodness"  of  the  bound  against  the  amount  of  computation  required 
to  obtain  it.  Thus,  we  need  a  method  to  obtain  a  bound  which  is  as  tight  as  possible,  but  which 
can  be  achieved  relatively  efficiently. 

The  efficiency  of  human  produced  solutions  to  similar  problems  stems  from  the  human  abil- 
ity to  group  many  distinct  points  into  homogeneous  regions  and  then  plan  paths  on  a  region  to 
region  basis.  The  resulting  paths  may  not  be  optimal,  but  they  are  reasonable  and  can  be 
obtained  efficiently.  Given  this  broad  characterization,  it  is  apparent  that  an  efficient,  human  like 
search  of  a  homogeneous  region  problem  representation  constitutes  a  suitable  vehicle  to  determine 
a   bound  for  the  propagation  techniques.   Optimality  is  not   a  requirement   of  the  bound.   An  effi- 


cicntly  produced,  reasonable  solution  is  the  goal. 

The  two  drawbacks  associated  with  the  extant  homogeneous  regions  method  are  the  need  to 
represent  regions  as  centrally  symmetric  figures  and  the  requirement  to  move  from  region  center 
to  region  center  First,  we  overcome  the  problem  associated  with  region  centers.  Consider  the 
problem  of  finding  the  optimal  path  in  a  simple  case.  Figure  1  depicts  a  situation  where  the 
requirement  is  to  move  from  an  initial  location  inside  a  "cheap"  region  to  a  goal  also  in  the 
"cheap"  region  when  there  is  a  rectangular  "expensive"  region  on  the  straight  line  path  between 
them.  As  in  the  illustration,  the  optimal  path  (the  solid  start  to  goal  line)  is  some  perturbation  of 
a  straight  line  which  trades  increased  distance  in  the  cheap  region  for  deceased  distance  in  the 
expensive  region  In  figure  1.  Si  and  S2  represent  cost  reciprocals.  I),  dl.  d2,  yl,  y'2  and  y3 
represent  Euclidian  distances.  Let  T  denote  the  cost  of  a  path  The  equation  describing  T  is  a  sum 
of  terms  as  below. 


V  d\2+y\2      \   (D-d  \-d2)2+yZ2       \rd22~y22 


We  wish  minimum  cost  paths  as  a  solution.  Thus,  we  take  the  first  derivative  of  T  and  set  the 
resulting  equations  equal  to  zero.  The  partial  derivative  of  T  with  respect  to  dl  and  set  equal  to 
yields: 

d  1  D -dl-d2 


SlVyl2+dl2      5  2v  yZ2^(D  -d\-d2)2 
In  a  similar  manner,  taking  the  partial  derivative  of  T  with  respect  to  d2  and  setting  the  equation 
equal  to  zero  yields: 

d2  D     d\-d2 


S\\  y22-d22      S2\yZ2~[D     d\     d2)2 
In  Figure  I.  let  Al.  A2  and  A3  represent  angular  measures    Note  that: 
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Then,  the  above  computations  simplify  to: 

sin  (A  1)       sin  (^4  3)       ein  (A  2) 
5  ]  5  3  5  1 

A  minimum  cost  path  is  the  result  of  satisfying  this  final  equation. 

We  note  that  the  final  equation  is  exactly  Snell's  Law  [Ref.  4]  commonly  used  in  optics  to 
compute  the  paths  of  light  rays  traveling  through  media  of  different  reflective  indices.  There  is  no 
known,  closed  form  equation  to  solve  a  Snell's  Law  problem.  Iteration  and  search  (such  as  bisec- 
tion search)  are  normally  used  to  find  optimal  paths.  Also.  Snell's  Law  is  known  to  express  an 
entirely  "local"  relationship,  that  is,  the  law  determines  the  optimal  path  based  solely  on  locally 
available  information  Thus,  the  homogeneous  regions  need  not  be  strictly  represented  by  rec- 
tangular figures,  any  linear  boundary  separating  two  regions  is  sufficient  to  support  application  of 
the  law.  This  characteristic  eliminates  the  difficulties  of  the  homogeneous  regions  method  associ- 
ated with  the  requirement  for  centrally  symmetric  figures.  Another  benefit  of  Snell's  Law  is  that 
the  optimal  path  can  be  computed  across  any  number  of  consecutive,  linear  boundaries.  Thus, 
homogeneous  regions  can  be  adjacent  to  each  other. 

The  locality  aspect  of  Snell's  Law  also  has  an  adverse  implication.  Consider  a  situation 
similar  to  that  depicted  by  Figure  2.  Let  region  C  be  the  most  favorable,  region  A  the  next  best 
and  region  B  be  the  least  favorable  region.  Snell's  Law  will  simply  perturb  the  original,  straight 
line  start  to  goal  path  until  an  optimal  path  involving  regions  C  and  B  is  determined  (as 
represented  by  the  solid  line).  The  law  has  no  ability  to  consider  alternate  paths  involving  region 
A.  even  though  the  optimal  path  could  easily  be  similar  to  the  dashed  line  path  of  Figure  2.  Thus, 
Snell's  Law  implicitly  relies  on  a  straight  line  hypothesis  to  propose  an  initial  route  The  law 
ignores  more  favorable,  adjacent  areas  unless  either  the  initial  straight  line  path  intersects  them 
or  the  optimization  procedure  accidentally  perturbs  the  path  into  an  intersection  with  them 

This  situation  can  lead  to  a  form  of  the  same  problem  that  effects  the  wavefront  propaga- 
tion technique,  i.e.  combinatorial  explosion  and  computational  excess.  Deriving  a  light  bound  by 
applying  Snell's  Law  to  a  large  number  of  distinct  regions  requires  that  each  region  within  some 
circumscribing  limit  be  specifically  examined.  The  problem  is  somewhat  less  serious  because  the 
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Figure  2 
Locality  Aspect  of  Snell's  Law 
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size  of  each  individual  area  requiring  examination  is  generally  much  larger  than  the  areas  used  in 
a  lattice  representation.  However,  if  a  path  involving  a  significant  distance  from  the  start  loca- 
tion to  the  goal  location  is  required,  the  size  of  the  areas  has  less  of  an  ameliorating  effect.  More- 
over, the  type  of  technique  we  are  attempting  to  develop  must  produce  a  bounding  solution  effi- 
ciently to  be  valuable  It  does  not  appear  that  a  method  which  must  "look  everywhere"  can  fulfill 
this  requirement. 

This  indicates  that  Snell's  Law  may  not  be  an  appropriate  formula  to  apply.  However,  aban- 
doning the  law  is  not  a  realistic  option  The  analysis  began  with  a  mathematical,  calculus  based 
description  of  a  minimum  cost  path  through  a  single  homogeneous  region.  The  solution  to  the 
calculus  problem  produced  Snell's  Law  as  a  result  Thus,  short  of  finding  another  mat  hematical 
basis  for  the  minimization  problem  itself,  it  seems  that  any  characterization  of  the  minimal  cost 
route  will  have  the  same  problems  as  Snell's  Law  Another  option  is  to  develop  a  heuristic  which 
allows  a  more  intelligent  representation  of  the  problem  space  so  that  Snell's  Law  will  be  applica- 
ble in  an  efficient  manner. 

To  derive  such  a  heuristic,  we  again  appeal  to  the  human  solution  model.  While  humans 
use  the  homogeneous  regions  approach,  they  seem  to  rely  on  a  very  small  number  of  region  group- 
ings Again  refer  to  Figure  1  where  Snell's  Law  works  perfectly.  There  are  only  three  possible 
types  of  regions  associated  with  this  situation,  optimal  regions,  impassable  (obstacle)  regions,  and 
regions  which  are  traversable,  but  not  at  optimal  cost.  The  use  of  only  three  region  groupings 
solves  the  problems  stemming  from  the  local  nature  of  Snell's  Law.  If  a  path  (or  path  segment) 
traverses  only  an  optimal  region,  there  is  no  need  to  "look"  elsewhere  for  a  possibly  better  solu- 
tion If  a  path  traverses  a  non-optimal  region,  it  is  only  necessary  to  examine  paths  through 
immediately  bordering  regions  (as  well  as  the  original  path  through  the  non-optimal  region) 
because  the  border  regions  must  be  optimal.  Thus,  a  ternary  representation  of  the  problem  space 
facilitates  the  application  of  Snell's  Law  This  representation  also  corresponds  to  a  human  like 
strategy  of  grouping  regions  as  either  the  best  (optimal),  the  worst  (obstacle),  or  somewhere  in 
between   (traversable,   but   not  at  optimal  cost).  The  ternary  grouping  may  not    produce  solutions 
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which   are  as  accurate  as  those  available  by   a  more  sophisticated   representation.     However,  this 
method  has  traded  exact  fidelity  for  efficiency 

Thus,  we  have  a  general  method  of  combining  human  like  efficiency  with  the  capability  of 
graph  theoretic  representations  to  achieve  optimal  solutions.  The  requirement  of  the  human  like 
planning  process  is  to  develop  "as  good  a  plan  as  possible"  within  the  efficiency  constraints.  The 
"goodness"  of  the  solution  determines  the  degree  of  constraint  imposed  on  the  efforts  required  by 
an  optimizing  graph  search  procedure.  We  have  developed  an  informal,  empirical  argument  which 
rejects  the  notion  of  using  many  distinct  region  classifications.  The  resulting  ternary  grouping 
scheme  is  critical  to  the  human  like  planning  model.  It  remains  to  justify  the  heuristic  assump- 
tion. 

3.1.  Selecting  a  Capability  Model 

The  importance  of  capability  knowledge  is  established  in  preceding  sections.  Without  such 
knowledge,  homogeneous  regions  cannot  be  created.  To  establish  capability  knowledge,  some 
specific  type  of  motion  achieving  entity  must  be  referenced  There  has  been  a  large  amount  of 
progress  towards  solving  the  extended  problem  for  wheeled  vehicles  !Ref.  5).  (We  note  that  the 
success  of  this  work  critically  relies  on  the  capabilities  of  wheeled  vehicles  relative  to  roadways  as 
traversable  media.)  This  being  given,  we  select  legged  entities  as  the  capability  model  for  this 
study.  This  class  contains  animals  (humans)  and  legged  autonomous  vehicles.  Further,  as  much 
path  planning  work  is  relevant  to  the  robotics  area,  we  select  legged  autonomous  vehicles  as  the 
specific  model.  This  subject  has  the  further  advantage  of  the  availability  of  precise  mathematical 
descriptions  of  motion.  Note  that  this  selection  does  not  cause  the  work  to  be  overly  specific  and 
non-generalizable.  A  different  selection  would  cause  a  new  definition  of  capability  constraints.  The 
general  solution  produced  should  still  be  applicable  to  planning  human  motion  as  well  as  planning 
the  location  of  static  linear  features  such  as  roadways  or  pipelines. 

3.2.  The  Ternary  Case  Assumption 

The   capabilities   of  a  specific    model   can    now    be   exploited      Legged,    autonomous  vehicles 
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have  characteristics  which  greatly  differentiate  them  from  wheeled  vehicles  [Ref.  9].  First,  road- 
ways do  not  exhaustively  define  the  set  of  most  favorable  terrain  features  for  traversability. 
Clearly,  using  a  human  model  for  illustration,  one  can  walk  across  an  open  field  as  easily  as  a 
road.  A  second  characteristic  of  legged  machines  is  that  their  total  range  of  attainable  speeds  is 
not  as  great  as  that  of  wheeled  vehicles.  Thus,  a  collection  of  terrain  features  grouped  into  a  sin- 
gle homogeneous  region  based  on  a  generalized  range  of  attainable  speeds  contains  more  members 
than  a  region  created  using  wheeled  vehicles  as  the  model.  Based  upon  these  two  general  capabil- 
ity traits,  as  well  as  the  discussions  concerning  Snell's  Law,  an  assumption  pertaining  to  the  crea- 
tion of  homogeneous  regions  is  appropriate.  Accordingly,  we  posit  the  ternary  case  assumption: 
all  physical  terrain  features  can  be  placed  into  one  of  three  disjoint  equivalence  classes  by  a 
traversability  relation  based  upon  legged  vehicle  capabilities.  The  first  class  contains  optimum 
(per  traversability)  features  such  as  roads,  open  fields,  non-wooded  areas  with  little  elevation 
change,  pastures  and  trails.  All  of  these  features  can  be  traversed  at  relatively  the  same  speed  by 
a  legged  machine  (much  as  they  can  be  by  a  walking  human).  The  second  class  contains  terrain 
features  which  are  traversable,  but  not  at  an  optimum  speed.  Typical  class  members  are  sparsely 
wooded  areas,  marsh  or  rocky  areas  with  marginal  footing,  areas  with  moderate  elevation  changes 
or  slopes  and  congested  areas,  such  as  townships.  The  final  class  contains  non-traversable  features. 
This  class  contains  such  features  as  rivers,  buildings,  densely  wooded  areas,  steep  mountains  and 
restricted  areas  With  minimum  time  paths  as  the  goal,  this  last  class  can  also  contain  some 
features  which  are  physically  traversable,  but  should  be  avoided  due  to  the  extreme  computation 
requirements  they  impose  on  the  robot's  local  sensor  systems.  A  moderately  dense  wooded  area  is 
an  example  of  such  a  feature.  (liven  that  features  can  be  recognized  (or  provided  a  prion),  an 
algorithmic  means  to  utilize  the  ternary  case  assumption  and  solve  the  extended  Find-Path  prob- 
lem is  presented. 

3.3.    Path  Planning  Through  Elliptical  Limiting 

An    algorithmic    means    of  solving    the    extended    Find-Path    problem    which    combines    the 
advantages   of  human   and    graph    theoretic    models   is   the  desired    goal     We   have   observed    that 
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human  solutions  are  efficient  while  graph  theoretic  methods  produce  optimality.  This  suggests  a 
natural  division  of  the  algorithmic  organization  into  two  primary  parts.  The  first  part  relies  upon 
human  inspired  traits  to  efficiently  produce  a  possibly  non-optimal  solution.  This  solution  is  used 
as  a  bound  to  constrain  the  efforts  of  a  second  stage  process  which  is  designed  to  ensure  optimal- 
ity.* 

The  initial  step  in  our  method  is  to  perform  an  opportunistic  decomposition  of  the  problem 
by  recognizing  corridors  through  impassable  regions.  A  knowledge  based  component  can  perform 
this  function.  Each  part  of  this  decomposition  can  be  solved  independently.  An  appropriate 
overall  solution  is  achieved  by  geometrically  linking  the  subproblem  solutions  together  at  corridor 
entry  exit  points  Since  each  subproblem  is  independent,  we  only  need  to  concern  ourselves  with 
the  solution  to  a  single  problem. 

The  second  step  solution  process  resembles  a  bidirectional  A*  search  [Ref.  10]  over  a  dynam- 
ically created  graph  and  uses  a  straight  line  hypothesis  to  guide  a  generate  and  test  methodology. 
A  homogeneous  region  representation  is  used  to  dynamically  construct  the  graph.  The  difficulties 
associated  with  interacting  subproblems  are  avoided  by  using  bidirectional  A*  search  and  selec- 
tively remembering  nonobstructed  links.  In  the  case  where  only  one  obstacle  lies  between  the 
start  and  goal  there  is  no  opportunity  for  subproblem  conflicts  to  occur.  Our  solution  technique 
utilizes  recursion  to  extend  this  situation  over  the  entire  graph. 

Our  method  operates  in  the  following  fashion.  First,  we  propose  a  straight  line  path  from 
start  to  goal.  If  a  collision  occurs  on  this  path,  we  determine  those  vertices  which  facilitate  move- 
ment around  the  associated  obstacle.  We  then  treat  each  obstacle  vertex  as  the  current  location 
and  generate  two  new  paths  from  this  point,  one  to  the  start  and  one  to  the  goal.  Similar  actions 
occur  when  traversable,   but   non-optimal  areas  are  intersected.  We  determine  the  vertices  which 


In  all  further  discussion  we  assume  that  terrain  features  have  been  recognized  and  grouped  according  to  the  ter- 
nary case  assumption.  Conceptually,  we  assume  the  existence  of  a  two  dimensional  plane  where  nonoptimal, 
traversable  class  features  and  obstacle  class  features  are  represented  as  convex  polygons.  In  this  system,  optimal 
class  features  are  not  specifically  presented  but  are  considered  to  be  the  "background"  over  which  the  polygons  are 
superimposed.  The  linear  sides  of  polygonal  figures  support  application  of  Snell's  Law.  Convexity  facilitates  locat- 
ing vertices  which  allow  movement  around  the  obstacle. 
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facilitate  movement  around  the  region  and  treat  these  as  new,  current  locations.  We  also  compute 
the  best  path  through  the  region  in  accordance  with  Snell's  Law.  This  path  terminates  at  the 
start,  the  goal,  or  another  region  intersection.  Eventually,  the  entire  process  finds  an  unob- 
structed link  set  to  both  the  start  and  goal.  At  this  point,  the  process  stores  only  those  links 
which  directly  connect  an  obstacle  vertex  to  either  the  start  or  goal.  Once  this  process  has  been 
completed,  all  the  "best"  links  going  into  the  start  and  into  the  goal  have  been  located.  We  then 
use  the  A*  estimate  function  to  approximate  the  cost  of  completing  the  middle  of  each  possible 
path  When  the  most  favorable  start  and  goal  link  pair  has  been  determined,  we  treat  the  interior 
endpoints  of  the  links  as  new  start  and  goal  locations  and  recursively  apply  the  same  technique. 
When  the  actual  cost  of  completing  the  most  favorable  partial  path  is  zero,  the  best  "straight 
line"  solution  has  been  found. 

The  above  process  requires  the  use  of  the  straight  line  hypothesis.  A  form  of  the  hypothesis 
based  on  the  ternary  case  assumption  is  available.  Clearly,  the  hypothesis  is  valid  inside  a  single 
homogeneous  region.  This  includes  the  case  of  going  around  obstacles  where  only  the  leftmost 
and  rightmost  (conceptually)  vertices  need  to  be  located.  The  difficulty  arises  when  passing 
between  slow  and  high  speed  traversable  areas.  In  this  case,  Snell's  law  can  be  applied  to  find  the 
optimal  entry  exit  point  on  the  slow  fast  region  boundary.  Once  Snell's  law  has  found  the  per- 
tinent boundary  points,  they  can  be  added  to  the  dynamically  created  graph  as  easily  as  obstacle 
vertices. 

Figures  3  through  5  illustrate  the  operation  of  the  second  step  process.  In  each  figure,  the 
original  start  is  located  at  the  origin.  The  goal  has  coordinates  (30,25).  The  filled  rectangles 
represent  obstacles.  The  single,  unfilled  rectangle  represents  a  non-optimal,  traversable  region.  In 
Figures  3  and  4.  the  dashed  lines  represent  unobstructed  links  to  the  start  while  the  solid  lines 
depict  similar  links  for  the  goal.  Figure  3  shows  the  solution  state  at  the  end  of  the  first  recursion 
where  the  start  and  goal  are  as  listed  above.  There  are  3  links  connected  to  the  goal  and  5  for  the 
start.  Of  the  start  links,  two  involve  the  non-optimal  region.  There  is  a  straight  line  segment  con- 
necting the  upper  left  vertex  of  this  region  to  the  start.  This  point  is  static.  The  second  link  is  a 
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Figure  3 
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Figure  4 
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Figure  5 
Resulting  Optimal  Path 
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path  perturbed  by  Snell's  Law  which  connects  the  start  to  an  obstacle  vertex  at  coordinates 
(10,20).  The  path  (and  path  cost)  is  static  as  are  the  endpoints.  Subsequent  paths  through  the 
region  require  new  computations.  As  an  example,  if  it  proves  desirable  to  move  from  the  vertex  at 
(4.6)  to  the  vertex  at  (10,20),  a  new  Snell's  Law  path  must  be  computed. 

At  the  end  of  the  first  recursion,  all  possible  pairs  of  start  links  and  goal  links  are  matched 
to  determine  the  most  promising  pair.  Note  that  two  complete  paths  already  exist.  However,  their 
costs  are  greater  than  the  actual  cost  of  the  (0,0)  to  (6.4)  link  plus  the  actual  cost  of  the  (30,25) 
to  (20.10)  link  plus  the  estimated  cost  of  the  uncompleted  portion  of  the  remaining,  middle  link. 
This  particular,  hypothetical  path  has  the  most  favorable  cost  estimate  of  any  link  pair.  Thus. 
(6.4)  is  selected  as  the  new  start  and  (20,10)  the  new  goal  to  be  used  in  the  second  recursion  (Fig- 
ure 4).  The  second  recursion  finds  a  complete  path  between  these  two  points  which  also  happens 
to  have  the  least  cost  estimate  of  any  pair  of  links  discovered  to  that  point.  Therefore,  the  process 
is  complete  and  the  path  1(0,0),  (6,4),  (11,9),  (20,10).  (30,25)]  is  returned  as  the  optimal  solution 
at  the  conclusion  of  two  recursive  applications  (see  Figure  5). 

The  second  step  portion  has  been  implemented.  The  execution  times  recorded  by  the 
method  compare  very  favorably  to  those  of  the  V'Graph  technique  in  the  solution  of  binary  case 
problems  (see  Table  l).  Both  algorithms  were  implemented  in  an  interpreted  version  of  Prolog  on 
a  time  shared  VAX  11/780  computer.  As  much  as  possible,  both  implementations  use  identical 
sections  of  code  for  low  level  functions  (such  as  line  intersection  checking).  Efforts  were  made  to 
insure  that  the  two  implementations  differed  in  control  strategy  only.  In  Table  1,  execution  times 
are  in  seconds  (wall  clock  time)  as  measured  by  a  Prolog  predicate.  Obstacles  were  added  to  a 
problem  one  at  a  time.  Each  algorithm  was  used  to  solve  the  problem  after  the  addition  of  a  sin- 
gle obstacle.  The  addition  of  obstacles  3  through  6  did  not  alter  the  optimal  path  This  is  reflected 
in  the  nearly  static  execution  times  of  the  step  two  algorithm.  (Decreases  are  due  to  fluctuation  in 
computer  loads.)  The  VGraph  execution  time  increases  are  due  to  the  need  to  build  ever  larger, 
exhaustive  graphs. 

The  result  of  the  second  step  process  is  a  reasonable,   perhaps  nonoptimal,  path  through  a 
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Table  1:  Execution  Time  Comparison 


Number 

of 

VGraph 

Step  Two 

Obstacl 

es 

Method 

Process 

1 

4.48 

4  1 

2 

7.28 

4.75 

3 

11.35 

4.65 

< 

16.37 

4.66 

5 

22.96 

4.63 

6 

32.33 

4.71 

I 

46.08 

6.32 

region  characterized  according  to  the  ternary  case  assumption.  The  process  relied  on  homogene- 
ous regions,  a  dynamically  created  graph,  guidance  by  a  form  of  the  straight  line  hypothesis,  plan- 
ning for  interaction  conflicts,  opportunistic  decomposition,  recursion  to  save  useful  results  and 
intelligent,  bidirectional  search.  The  solution  has  been  achieved  relatively  quickly  and  wasteful 
computations  have  been  avoided.  It  remains  to  ensure  optimality. 

The  third  step  of  the  elliptical  limiting  method  is  designed  to  provide  an  optimal  solution. 
Here,  a  lower  level  of  abstraction  is  appropriate.  Assume  that  the  homogeneous  regions  format 
can  be  converted  (or  is  otherwise  available)  to  a  lattice  representation  such  as  that  used  in  the 
wavefront  propagation  methods.  Given  this  representation,  the  solution  obtained  in  the  second 
step  can  be  used  as  a  bound  to  constrain  the  efforts  required  for  a  wavefront  propagation  tech- 
nique. (For  simplicity  of  discussion,  assume  that  time  is  the  factor  to  be  optimized.)  Any  optimal 
solution  must  have  a  cost  less  than  or  equal  to  that  required  by  the  second  step  solution  The 
time  cost  of  that  solution  is  known  We  compute  the  greatest  distance  that  can  be  traveled  in  an 
equal  amount  of  time  if  only  the  fastest  speed  regions  are  traversed.  We  then  construct  an  ellipse 
as  discussed  previously.  Any  optimal  solution  must  lie  entirely  within  this  ellipse.  The  number  of 
points  interior  to  the  ellipse  can  be  reduced  by  eliminating  any  points  belonging  to  the  obstacle 
class  as  defined  in  the  homogeneous  region  classification.  We  then  conduct  a  wavefront  propaga- 
tion over  the  lattice  of  remaining  points  to  find  the  optimal  solution. 

The  third  step  process  results  in  a  set  of  start  to  goal  paths  which  all  require  the  same 
amount   of  traversal  time    The  final  step  in  the  elliptical  limiting  process  determines  the  actual, 
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optimal  path  from  this  set.  The  wavefront  technique  implemented  at  Hughes  Research  jRef.  6] 
requires  heuristic  measures  to  be  applied  to  this  set  of  paths  so  that  the  digital  representation  of 
the  true  optimal  path  can  be  determined.  Elliptical  limiting  need  not  resort  to  complex,  heuristic 
measures.  The  ternary  case  assumption  requires  that  the  environment  be  represented  as  contigu- 
ous areas  of  homogeneous  regions.  Before  the  ternary  grouping  can  be  made,  the  boundaries  of  all 
areal  features  in  the  environment  must  be  recognized.  Note  that  in  an  optimal  path,  turn  points 
only  occur  at  feature  boundaries  because  within  a  single  region,  the  least  cost  path  is  always  a 
straight  line.  To  find  the  optimal  solution,  we  superimpose  the  solutions  found  in  the  third  step 
process  over  the  original  (unprocessed,  analog  map  feature  image)  problem  representation.  Any 
point  in  the  third  step  solution  set  not  corresponding  to  a  boundary  crossing  can  be  eliminated.  In 
this  representation,  the  single  optimal  path  can  be  easily  separated  from  the  set  of  solutions  gen- 
erated by  the  third  step  process.  Moreover,  the  accumulation  of  error  present  in  "stairstep"  digital 
representations  of  straight,  diagonal  lines  is  eliminated. 

Figures  6  through  9  illustrate  the  entire  elliptical  limiting  process.  Figure  6  presents  an 
unprocessed  representation  of  the  physical  world  to  be  traversed.  For  illustrative  simplicity,  the 
problem  contains  only  optimal  and  obstacle  class  regions.  The  optimal  regions  are  composed  of 
roadways  and  open,  flat  fields.  Obstacle  class  features  include  a  river  and  several  buildings  (the 
checkerboard  filled  rectangles).  Figure  7  illustrates  the  initial  step  of  the  process.  The  optimal 
path  must  cross  the  bridge  over  the  river.  The  result  is  an  opportunistic  decomposition  which 
includes  the  formulation  of  additional  start  and  goal  locations.  There  is  a  new  goal  in  the  lower 
subproblem  and  a  new  start  for  the  upper  subproblem.  The  dashed  line  through  the  river 
represents  the  decomposition  boundary.  Figure  8  depicts  the  second  step  solution  to  the  lower 
subproblem.  The  shaded  areas  are  obstacle  regions.  The  background  is  the  optimal  region.  This 
situation  is  overly  simplified  by  the  absence  of  non-optimal  traversable  regions.  In  fact,  the 
absence  of  such  areas  results  in  a  simple  binary  problem  for  which  the  second  step  process  will 
produce  the  exact,  optimal  solution.  Figure  9  depicts  the  ellipse  produced  from  the  second  step 
solution  and  the  optimal  path  found  by  the  graph  search  performed  as  the  third  step.  The  final 
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Figure  6 
Analog  Map  Representation 
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Figure  7 
Problem  Decomposition 
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Figure  8 
Second  Step  Solution 
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Figure  9 

Third  Step  Solution  and 
Limiting  Ellipse 


Note  that  the  darkened  portions  of  the  ellipse 

are  not  included  in  the  corresponding  graph.  These 

locations  are  within  obstacles 
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step  requires  that  the  set  of  optimal  paths  found  in  the  graph  search  be  overlayed  on  the  original 
problem  representation*.  The  only  turn  point  in  the  path  produced  by  the  graph  search  which 
lies  on  a  feature  boundary  corresponds  to  the  obstacle  vertex  considered  by  the  second  step  pro- 
cess. Thus,  the  optimal  path  contains  the  start  location,  a  single  turn  point,  and  the  goal  location 
This  path  is  exactly  the  route  depicted  in  Figure  7,  the  second  step  solution. 

3.4.    Summary 

We  have  developed  and  presented  a  new  method  to  solve  the  extended  Find-Path  problem 
Our  system  has  individual  traits  which  can  be  found  in  other  paradigms.  However,  the  similarities 
to  any  other  single  method  end  at  this  low  level.  No  system  that  we  have  examined  resembles  our 
technique  in  total  philosophy  or  methodology.  We  propose  that  this  comment  can  be  extended  to 
include  efficiency  of  operation.  The  extant  extended  case  solution  methods  exhibit  one  or  more  of 
several,  salient  deficiencies  Either  they  fail  to  produce  optimal  solutions,  they  are  excessive  com- 
putationally, or  they  require  heuristic,  ancillary  operations  to  determine  the  optimal  path  from  a 
set  of  solutions.  Our  system  has  none  of  these  deficiencies. 

Our  system  also  includes  new  techniques  (per  the  path  planning  problem).  We  have  com- 
bined human  like  capabilities  with  those  of  a  machine.  We  have  focused  on  discovering  the 
underlying,  enabling  principles  which  allow  human  efficiency  and  then  employing  these  principles. 
Also,  our  system  includes  a  ternary  problem  representation  and  an  efficient  method  to  solve  this 
case  Previously,  only  "intelligent"  solutions  to  binary  case  problems  or  "brute  force"  techniques 
for  the  n-ary  case  problems  have  been  available.  Also,  this  solution  involves  the  implementation 
of  a  bidirectional  A*  search.  Further,  previous  efforts  either  resulted  in  an  intelligent  strategy 
suitable  for  single  processor  machines  or  a  brute  force  method  amenable  to  parallel  execution  Our 
technique  includes  both  principles**.  It  features  an  intelligent,  second  step  strategy  usable  on 
uniprocessor    architectures   and    a   constrained    third    step   process   which    can    be    implemented    on 


Note  thai,  for  clarity,  only  a  single  path  is  depicted  in  Figure  9.    Presenting  the  entire  set  of  digitally  equivalent 
paths  within  the  small  ellipse  hinders  the  explanatory  effects  of  the  figure. 

Thi    wavefront   propagation   technique,  as  a  general  method,  has  been   implemented  on   parallel  machines    Ref.   12. 
We  rely  on  this  demonstrated  capability  of  the  algorithm  to  claim  suitability  for  multiprocessors. 
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multiprocessors.  Our  process  is  also  flexible.  If  timeliness  is  the  paramount  concern,  only  the  first 
two  steps  need  be  invoked  to  produce  a  quick,  reasonable  solution  When  optimality  must  be 
ensured,  all  steps  can  be  utilized 

In  summary,  we  have  presented  a  solution  technique  which  involves  mutually  cooperating 
processes:  each  serves  to  ameliorate  the  poor  performance  aspects  of  the  others.  The  first  two 
steps  are  efficient  and  need  not  be  overly  concerned  with  optimality  (much  as  is  the  human  path 
planning  process).  The  third  step  ensures  optimality  by  searching  a  space  which  has  been  greatly 
pruned  by  the  efforts  of  the  earlier  processes.  Further,  the  better  the  result  of  the  second  step,  the 
less  effort  required  in  the  third  step.  Finally,  the  last  step  determines  the  true  optimal  path 
without  resort  to  heuristic,  ancillary  operations  Our  plans  for  the  future  include  an  implementa- 
tion of  the  entire  method  so  that  it  may  be  measured  against  competing  processes  as  well  as 
diverse  problem  representations 
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